import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;


/**Procedurale WallFollower **/

public class WallFollower {
	private static final boolean VERBOSE = false;
	private static int sleepTime = 100;

	public static void main(String [] args) throws InterruptedException{		
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 164;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		TachoPilot pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		UltrasonicSensor sonicSensor = new UltrasonicSensor(SensorPort.S2);
		
		TouchSensor touchSensor = new TouchSensor(SensorPort.S4);
		
		int distanceFromWall = 17;
		int offset = 3;
		float angle = 20;
		int correctionDistance = 50;
		int bumpAngle = 20;
		
		
		while(true){
			LCD.clearDisplay(); LCD.drawString("Dist:" + sonicSensor.getDistance(), 0, 0);
			//sleep(sleepTime);
			if(Button.ENTER.isPressed()){
				sonicSensor.reset();
				LCD.drawString("RESET",0,3);
			}
			if(touchSensor.isPressed()){	//Tegen de muur gereden			
				correctBump(pilot, sonicSensor, bumpAngle);				
			}else if (!binnenInterval(sonicSensor, distanceFromWall, offset)){	//Niet de muur aan het volgen		
				if(sonicSensor.getDistance() == 255){
					LCD.drawString("Error?", 0, 0);
					break;
				}
				if(teVer(sonicSensor, distanceFromWall, offset)){
					correctTooFar(pilot, angle, correctionDistance);
				}else if(teDicht(sonicSensor, distanceFromWall, offset)){
					correctTooClose(pilot, angle, correctionDistance);
				}
			}else{ //Goe bezig!
				LCD.drawString("Juist", 0, 1);
				pilot.travel(20);
			}
		}		
	}
	private static void correctBump(TachoPilot pilot, UltrasonicSensor sonicSensor, int bumpAngle) throws InterruptedException {
		LCD.drawString("Bump!", 0, 1); //Thread.sleep(500);
		pilot.travel(-80);
		int originalDistance = sonicSensor.getDistance();
		pilot.rotate(90);
		if(sonicSensor.getDistance() > originalDistance){
			pilot.rotate(-180);
		}
	}
	private static void correctTooClose(TachoPilot pilot, float angle,
			int correctionDistance) throws InterruptedException {
		LCD.drawString("Te Dicht!", 0, 1); Thread.sleep(500);
		pilot.rotate(angle);
		pilot.travel(correctionDistance);
		pilot.rotate(-angle);
	}
	private static void correctTooFar(TachoPilot pilot, float angle,
			int correctionDistance) throws InterruptedException {
		LCD.drawString("Te Ver!", 0, 1); Thread.sleep(500);
		pilot.rotate(-angle);
		pilot.travel(correctionDistance);
		pilot.rotate(angle);
	}
	public static void TestPosition(TachoPilot pilot, UltrasonicSensor sensor){
		
	}
	
	private static boolean binnenInterval(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean kleinerDanMax = sonicSensor.getDistance() < distanceFromWall + offset;
		//sleep(sleepTime);
		boolean groterDanMin = sonicSensor.getDistance() > distanceFromWall - offset;
		//sleep(sleepTime);
		return kleinerDanMax && groterDanMin;
	}
	
	private static boolean teVer(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean teVer =sonicSensor.getDistance() >= distanceFromWall + offset;
		//sleep(sleepTime);
		return teVer;
	}
	
	private static boolean teDicht(UltrasonicSensor sonicSensor, int distanceFromWall, int offset){
		boolean teDicht = sonicSensor.getDistance() <= distanceFromWall - offset;
		//sleep(sleepTime);
		return teDicht;
	}
	
	private static void sleep(int milisec){
		try {
			Thread.sleep(milisec);
		} catch (InterruptedException e) {
			System.out.println("error bij sleep.");
		}
	}
}





/** 
 * OUDE CODE sonar rechts
 * currentDistance = sonicSensor.getDistance();
				LCD.drawString("afstandMr" + currentDistance, 0, 0);
				if(currentDistance < distanceFromWall){
					LCD.drawString("Draai naar rechts", 0, 3);
					pilot.rotate(angle);
					Thread.sleep(200);					
					float adjustDistance = (float) (currentDistance/(Math.abs((Math.sin(angle)))));
					LCD.drawString("AfstandAfL" + adjustDistance, 0, 1);
					pilot.travel(adjustDistance);
					Thread.sleep(200);
					pilot.rotate(-angle);
					Thread.sleep(200);
				}else{
					LCD.drawString("Draai naar links", 0, 3);
					pilot.rotate(-angle);
					Thread.sleep(200);					
					float adjustDistance = (float) (currentDistance/(Math.abs((Math.sin(angle)))));
					LCD.drawString("AfstandAfR" + adjustDistance, 0, 2);
					pilot.travel(adjustDistance);
					Thread.sleep(200);
					pilot.rotate(angle);
				}
**/
